#pragma once

#include "hnurm_dart/Detector.h"

#include <hnurm_interfaces/msg/vision_recv_data.hpp>
#include <hnurm_interfaces/msg/vision_send_data.hpp>


#include <image_transport/image_transport.hpp>
#include <sensor_msgs/msg/image.hpp>

#include <std_msgs/msg/float64.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>


// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>

// STD
#include <cmath>
#include <memory>
#include <vector>

namespace hnurm
{


class DetectNode : public rclcpp::Node
{
public:
    explicit DetectNode(const rclcpp::NodeOptions &options);

    void init_params();

private:
    void detect_callback(
        sensor_msgs::msg::Image::SharedPtr img_msg, hnurm_interfaces::msg::VisionRecvData::SharedPtr uart_msg
    );
    void image_callback( sensor_msgs::msg::Image::SharedPtr img_msg);

private:
    using ApproximateTimePolicy = message_filters::sync_policies::
        ApproximateTime<sensor_msgs::msg::Image, hnurm_interfaces::msg::VisionRecvData>;
    // subs
    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>               sub_img_;
    std::shared_ptr<message_filters::Subscriber<hnurm_interfaces::msg::VisionRecvData>> sub_uart_;
    std::shared_ptr<message_filters::Synchronizer<ApproximateTimePolicy>>               sync_;

    image_transport::Publisher                                      pub_res_img_;
    image_transport::Publisher                                      pub_bin_img_;
    // debug pubs
    bool pub_debug_info_ = true;
    bool use_uart = true;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_detect_time_;
    rclcpp::Publisher<hnurm_interfaces::msg::VisionSendData>::SharedPtr pub_send_data_;
    // std::shared_ptr<Detector>  detector_;
    Detector detector_;
    // topics
    std::string recv_topic_;
    std::string image_topic_;
    std::string res_img_topic_;
    std::string bin_img_topic_;
    std::string send_topic_;
    float bias = 0;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;

};
}
